#pragma once
#ifndef _MAP_H_
#define _MAP_H_

#include "common_include.h"
#include "frame.h"
#include"utility.h"
#include"backend.h"

namespace vld_imu_slam{

class Map {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    typedef shared_ptr<Map> Ptr;
    typedef unordered_map<unsigned long, Frame::Ptr> KeyFramesType;

    KeyFramesType keyframes_;

    Frame::Ptr p_current_frame_ = nullptr;  //由于插入关键帧不一定都是新帧，因此，索引关键帧vector的话，不一定能找到 

    Map(){
        p_backend_.reset(new Backend);
        pc_pose_.reset(new pcl::PointCloud<PointType>);
        p_kdtree_pose_.reset(new pcl::KdTreeFLANN<PointType>);
    }

    void insertKeyFrame(Frame::Ptr p_frame);

    pcl::PointCloud<PointType>::Ptr getCornerLocalMap();

    pcl::PointCloud<PointType>::Ptr getSurfLocalMap();

    pcl::PointCloud<PointType>::Ptr getMap();

    void updateLocalmap();

    void loopThread();

private:
    int num_active_keyframes_=3;

    pcl::PointCloud<PointType>::Ptr corner_localmap_,surf_localmap_;

    Backend::Ptr p_backend_;
    mutex keyframe_mutex_;
    pcl::PointCloud<PointType>::Ptr pc_pose_;
    pcl::KdTreeFLANN<PointType>::Ptr p_kdtree_pose_;

    vector<LoopFactor> loopfactor_v_;

    void searchLocalFrame(Frame::Ptr p_frame,vector<int>& point_search_Idx,double radius);

    //backend
    
};


}
#endif



